#ifndef __DUBINS_CURVE_H__
#define __DUBINS_CURVE_H__

#include <iostream>
#include <vector>
#include <math.h>
using namespace std;

// https://github.com/AndrewWalker/Dubins-Curves/

typedef enum 
{
    LSL = 0,
    LSR = 1,
    RSL = 2,
    RSR = 3,
    RLR = 4,
    LRL = 5
} DubinsPathType;

#define EDUBOK        (0)   // 没有错误，表示操作成功
#define EDUBCOCONFIGS (1)   // 配置共存冲突
#define EDUBPARAM     (2)   // 路径参数化错误
#define EDUBBADRHO    (3)   // rho 值无效
#define EDUBNOPATH    (4)   // 没有连接路径

typedef enum 
{
    L_SEG = 0,
    S_SEG = 1,
    R_SEG = 2
} SegmentType;

typedef struct 
{
    // (x, y, theta)
    vector<double> qi = {0.0,0.0,0.0};        
    /* the lengths of the three segments */
    vector<double> param = {0.0,0.0,0.0};     
    /* model forward velocity / model angular velocity */
    double rho;          
    /* the path type described */
    DubinsPathType type; 
} DubinsPath;

const SegmentType DIRDATA[][3] = {
    { L_SEG, S_SEG, L_SEG },
    { L_SEG, S_SEG, R_SEG },
    { R_SEG, S_SEG, L_SEG },
    { R_SEG, S_SEG, R_SEG },
    { R_SEG, L_SEG, R_SEG },
    { L_SEG, R_SEG, L_SEG }
};

typedef struct 
{
    double alpha;
    double beta;
    double d;
    double sa;
    double sb;
    double ca;
    double cb;
    double c_ab;
    double d_sq;
} DubinsIntermediateResults;

int DubinPath(vector<double> start, vector<double> end, double rho, vector<vector<double>>& poses);

#endif
